%   clear; 
%   clc;
function K = LQR_k_calc(leg_length)
% leg_length=0.05;
% 模型参数初始化
m=0.178; %车轮的质量 kg
r = 0.03375; %车轮的半径 m
M = 0.87486; %车体的质量 kg
I = 0.5*m*r^2; %车轮的转动惯量
l = leg_length; %质心距底盘中心的距离
Jz = 0.001476; %车体绕质心转动时的转动惯量
g=9.8;                     %重力加速度，单位为m/s^2

%状态空间参数初始化
a=r*(M+2*m+2*I/(r^2));
b=M*r*l;
c=Jz+M*l^2;
d=M*g*l;
e=M*l;
A23=-b*d/(a*c-b*e);
A43=a*d/(a*c-b*e);
B21=(c+b)/(a*c-b*e);
B22=(c+b)/(a*c-b*e);
B41=-(e+a)/(a*c-b*e);
B42=-(e+a)/(a*c-b*e);

%状态空间矩阵
A=[0 1  0  0 ;
   0 0 A23 0 ;
   0 0  0  1;
   0 0 A43 0 ]  ;                      %状态矩阵

B=[0 0;
    B21 B22;
    0 0;
    B41 B42];     %输入矩阵

%系统可控性判断
% Co=ctrb(A,B);
% if(rank(Co)==4)
%     disp('系统可控');
% else
%     disp('系统不可控');
% end

%LQR控制器设计
Q = [0.03 0  0    0 ;
        0   0.3 0   0 ; 
        0   0 100 0 ; 
        0   0  0    1 ]; %权重矩阵 Q 的设计

R = [140 0; 0 140]; %权重矩阵 R 的设计
K=lqr(A,B,Q,R) ;                      %调节参数

end



